#include "BulletPhysics.h"
#include "MyGL.h"
#include <iostream>

namespace Engine
{
	BulletPhysics::BulletPhysics()
	{
		m_dynamicWorld		= 0;
		m_collConfig		= 0;
		m_dispatcher		= 0;
		m_broadPhase		= 0;
		m_constraintSolver	= 0;

		red = 0;
	}

	void BulletPhysics::Initialize()
	{
		m_collConfig		= new btDefaultCollisionConfiguration();
		m_dispatcher		= new btCollisionDispatcher(m_collConfig);
		m_broadPhase		= new btDbvtBroadphase();
		m_constraintSolver	= new btSequentialImpulseConstraintSolver();

		m_dynamicWorld		= new btDiscreteDynamicsWorld(	m_dispatcher,
															m_broadPhase,
															m_constraintSolver,
															m_collConfig);
		m_dynamicWorld->setGravity(btVector3(0,-10,0));

		CreatePlane(Vec3(0.0f, 0.0f, 0.0f));
	}

	btRigidBody* BulletPhysics::CreatePlane(const Vec3& pos)
	{
		btTransform t;
		t.setIdentity();
		t.setOrigin(btVector3(0.0f, -0.1f, 0.0f));

		btScalar mass(0.0f);

		btStaticPlaneShape* plane = new btStaticPlaneShape(btVector3(0, 1, 0), 0);

		btMotionState* motion = new btDefaultMotionState(t);
		btRigidBody::btRigidBodyConstructionInfo info(mass, motion, plane);
		m_plane = new btRigidBody(info);

		m_dynamicWorld->addRigidBody(m_plane);

		return m_plane;
	}

	void BulletPhysics::RenderPlane(btRigidBody* body)
	{
        btTransform t;
        body ->getMotionState()->getWorldTransform(t);

        float mat[16];
        t.getOpenGLMatrix(mat);
		glColor3f(red, 0.1f, 0.1f);
        glPushMatrix();
                glMultMatrixf(mat);     //translation,rotation
				glEnable(GL_DEPTH_TEST);
                glBegin(GL_QUADS);
                        glVertex3f(-10.0f, 0.0f,  10.0f);
                        glVertex3f(-10.0f, 0.0f, -10.0f);
                        glVertex3f( 10.0f, 0.0f, -10.0f);
                        glVertex3f( 10.0f, 0.0f,  10.0f);
                glEnd();
        glPopMatrix();
	}

	void BulletPhysics::Update(const Vec3& n, const Vec3& f)
	{
		m_dynamicWorld->stepSimulation(1/60.0);
		
		rayFrom = btVector3(n.x, n.y, n.z);
		rayTo = btVector3(f.x, f.y, f.z);

		btCollisionWorld::ClosestRayResultCallback rcb(rayFrom, rayTo);
		m_dynamicWorld->rayTest(rayFrom, rayTo, rcb);

		if(rcb.hasHit())
		{
			red = 0.0f;

			p = (rayFrom + (rayTo*rcb.m_closestHitFraction));

			//std::cout << "px: " << p.getX() << std::endl;
			//std::cout << "py: " << p.getY() << std::endl;
			//std::cout << "pz: " << p.getZ() << std::endl;
		}
	}

	void BulletPhysics::Draw()
	{
		RenderPlane(m_plane);

		glPushMatrix();
			glBegin(GL_LINES);
				glColor3f(0.0f, 0.0f, 0.0f);
				glVertex3f(rayFrom.getX(), rayFrom.getY(), rayFrom.getZ());
				glVertex3f(p.getX(), p.getY(), p.getZ());
			glEnd();
		glPopMatrix();
	}

	void BulletPhysics::Shutdown()
	{
		m_dynamicWorld->removeCollisionObject(m_plane);
		btMotionState* motion = m_plane->getMotionState();
		btCollisionShape* shape = m_plane->getCollisionShape();

		delete m_plane;
		delete shape;
		delete motion;

		if(m_dynamicWorld)
			delete m_dynamicWorld;

		if(m_constraintSolver)
			delete m_constraintSolver;

		if(m_broadPhase)
			delete m_broadPhase;

		if(m_dispatcher)
			delete m_dispatcher;

		if(m_collConfig)
			delete m_collConfig;
	}
}